
package com.csjbot.snowbot_rogue.servers.slams.agent;

import android.graphics.RectF;

import com.csjbot.csjbase.log.Csjlogger;
import com.csjbot.snowbot_rogue.servers.slams.agent.Util.FileUtil;
import com.csjbot.snowbot_rogue.servers.slams.agent.Util.MapBean;
import com.csjbot.snowbot_rogue.servers.slams.data.MapDataCache;
import com.csjbot.snowbot_rogue.servers.slams.events.ArriveHomeEvet;
import com.csjbot.snowbot_rogue.servers.slams.events.ConnectedEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.ConnectionLostEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.LaserScanUpdateEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.MapOperationEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.MapUpdateEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.RobotHealthInfoEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.RobotStatusUpdateEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.WallUpdateEvent;
import com.slamtec.slamware.AbstractSlamwarePlatform;
import com.slamtec.slamware.SlamwareCorePlatform;
import com.slamtec.slamware.action.ActionStatus;
import com.slamtec.slamware.action.IAction;
import com.slamtec.slamware.action.IMoveAction;
import com.slamtec.slamware.action.MoveDirection;
import com.slamtec.slamware.action.Path;
import com.slamtec.slamware.discovery.DeviceManager;
import com.slamtec.slamware.geometry.Line;
import com.slamtec.slamware.robot.ArtifactUsage;
import com.slamtec.slamware.robot.HealthInfo;
import com.slamtec.slamware.robot.LaserScan;
import com.slamtec.slamware.robot.Location;
import com.slamtec.slamware.robot.Map;
import com.slamtec.slamware.robot.MapKind;
import com.slamtec.slamware.robot.MapType;
import com.slamtec.slamware.robot.MoveOption;
import com.slamtec.slamware.robot.Pose;
import com.slamtec.slamware.robot.Rotation;

import org.greenrobot.eventbus.EventBus;

import java.util.List;
import java.util.Vector;


public class RPSlamwareSdpAgent {
    private final static float SLAMWARE_MAX_UPDATE_MAP_RADIX = 7.0f;

    private AbstractSlamwarePlatform robotPlatform_;

    public SlamwareCorePlatform getRobotSlamPlatform() {
        return robotSlamPlatform_;
    }

    private SlamwareCorePlatform robotSlamPlatform_;
    private MapDataCache mapData_;
    private MapDataCache sweepMapData_;

    private Pose robotPose_;
    private LaserScan laserScan_;
    private IMoveAction moveAction_;
    private Vector<Line> walls_;
    private List<Location> circle_;
    private int wallId_;
    private int rotateDegree;
    private int rotateToDegree;

    private MapType mapType_;
    private RectF mergedMapUpdateArea_;
    private Rotation targetRotation_;
    private Vector<Line> wallsToAdd_;

    private String sdpVersion_;

    private boolean updatingPose_;
    private boolean updatingStatus_;
    private boolean updatingLaserScan_;
    private boolean updatingMoveAction_;
    private boolean updatingWalls_;
    private boolean rotating_;


    private String ip_;
    private int port_;
    private Location location_;
    private Pose pose_;
    private MoveDirection direction_;
    private Rotation targetRotate_;
    private boolean isUpdateMapping_;

    /**
     * 是否在虚拟轨道上循环
     */
    private boolean circleRunWithVt_;

    private boolean shouldReadLocalizationQuliaty = false;

    private Worker worker;

    private static boolean connected_;

    private static JobConnect jobConnect;
    private static JobAddWalls jobAddWalls;
    private static JobCancelAllActions jobCancelAllActions;
    private static JobClearMap jobClearMap;
    private static JobDisconnect jobDisconnect;
    private static JobGoHome jobGoHome;
    private static JobClearWalls jobClearWalls;
    private static JobMoveBy jobMoveBy;
    private static JobMoveTo jobMoveTo;
    private static JobMoveToPose jobMoveToPose;
    private static JobSweepSpot jobSweepSpot;
    private static JobRemoveWallById jobRemoveWallById;
    private static JobRotateTo jobRotateTo;
    private static JobGoHomePose jobGoHomePose;
    private static JobStartSweep jobStartSweep;
    private static JobUpdateLaserScan jobUpdateLaserScan;
    private static JobUpdateMap jobUpdateMap;
    private static JobUpdateMoveAction jobUpdateMoveAction;
    private static JobUpdatePose jobUpdatePose;
    private static JobUpdateStatus jobUpdateStatus;
    private static JobUpdateWalls jobUpdateWalls;
    private static JobUpdateWholeMap jobUpdateWholeMap;
    private static JobGetRobotHealth jobGetRobotHealth;
    private static JobSaveMap jobSaveMap;
    private static JobRegainMap jobRegainMap;
    private static JobCircleRun jobCircleRun;
    private static JobRotate jobRotate;
    private static JobAutoUpdateMapping jobAutoUpdateMapping;


    private static RPSlamwareSdpAgent rpSlamwareSdpAgent = null;

    public RPSlamwareSdpAgent() {
        mapData_ = new MapDataCache();
        sweepMapData_ = new MapDataCache();

        mergedMapUpdateArea_ = new RectF(0, 0, 0, 0);
        robotPose_ = new Pose(new Location(0, 0, 0), new Rotation(0, 0, 0));
        port_ = 0;
        updatingPose_ = false;
        updatingStatus_ = false;
        updatingLaserScan_ = false;
        updatingMoveAction_ = false;
        updatingWalls_ = false;
        rotating_ = false;
        wallsToAdd_ = new Vector<>();

        sdpVersion_ = "";
        jobConnect = new JobConnect();
        jobAddWalls = new JobAddWalls();
        jobCancelAllActions = new JobCancelAllActions();
        jobClearMap = new JobClearMap();
        jobDisconnect = new JobDisconnect();
        jobGoHome = new JobGoHome();
        jobClearWalls = new JobClearWalls();
        jobMoveBy = new JobMoveBy();
        jobMoveTo = new JobMoveTo();
        jobMoveToPose = new JobMoveToPose();
        jobCircleRun = new JobCircleRun();

        jobRegainMap = new JobRegainMap();
        jobSweepSpot = new JobSweepSpot();
        jobRemoveWallById = new JobRemoveWallById();
        jobRotateTo = new JobRotateTo();
        jobStartSweep = new JobStartSweep();
        jobUpdateLaserScan = new JobUpdateLaserScan();
        jobUpdateMap = new JobUpdateMap();
        jobUpdateMoveAction = new JobUpdateMoveAction();
        jobUpdatePose = new JobUpdatePose();
        jobUpdateStatus = new JobUpdateStatus();
        jobUpdateWalls = new JobUpdateWalls();
        jobUpdateWholeMap = new JobUpdateWholeMap();
        jobGetRobotHealth = new JobGetRobotHealth();
        jobGoHomePose = new JobGoHomePose();
        jobSaveMap = new JobSaveMap();
        jobRotate = new JobRotate();
        jobAutoUpdateMapping = new JobAutoUpdateMapping();
        worker = new Worker();

        Csjlogger.info("RPSlamwareSdpAgent init");
    }

    private static RPSlamwareSdpAgent agent = null;


    public MapDataCache getMapData() {
        return mapData_;
    }

    public MapDataCache getSweepMapData() {
        return sweepMapData_;
    }

    public boolean isConnected() {
        return connected_;
    }

    // robot action info
    public synchronized Pose getRobotPose() {
        return robotPose_;
    }
//

    public synchronized LaserScan getLaserScan() {
        return laserScan_;
    }

    public synchronized IMoveAction getMoveAction() {
        return moveAction_;
    }

    public synchronized Vector<Line> getWalls() {
        return walls_;
    }

    public String getSDPVersion() {
        return sdpVersion_;
    }

    public void saveMap() {
        pushJob(jobSaveMap);

    }

    public void circleRun(List<Location> list, boolean circleRunWithVt) {
        if (list != null) {
            synchronized (this) {
                circleRunWithVt_ = circleRunWithVt;
            }
            circleRun(list);
        }
    }

    public boolean isCircleRunning() {
        return isCircleRunning;
    }

    public void circleRun(List<Location> list) {
        if (list != null) {
            synchronized (this) {
                circle_ = list;
                isCircleRunning = true;
            }
        }
        pushJob(jobCircleRun);
    }

    // connection
    public void connectTo(String ip, int port) {
        Csjlogger.debug("connect to {}  port {}", ip, port);
        this.ip_ = ip;
        this.port_ = port;
        pushJob(jobConnect);
    }


    public void reconnect() {
        String ip;
        int port;

        synchronized (this) {
            ip = this.ip_;
            port = this.port_;
        }

        if (ip.isEmpty() || port == 0) {
            return;
        }
        connectTo(ip, port);
    }


    public void disconnect() {
        pushJob(jobDisconnect);
    }

    public void setAutoUpdateMap(boolean isUpdateMapping) {
        synchronized (RPSlamwareSdpAgent.class) {
            isUpdateMapping_ = isUpdateMapping;
        }
//       pushJob(jobAutoUpdateMapping);
    }

    // robot map info
    public void updateMap(RectF area) {
        if (area.isEmpty()) {
            return;
        }

        synchronized (this) {
            if (mergedMapUpdateArea_ != null && !mergedMapUpdateArea_.isEmpty()) {
                mergedMapUpdateArea_.union(area);
                return;
            } else {
                mergedMapUpdateArea_ = area;
            }
        }
        pushJob(jobUpdateMap);
    }

    //恢复地图
    public void regainMap() {
        pushJob(jobRegainMap);

    }

    public void getMap() {
        if (robotPlatform_ != null) {
            return;
        }
        robotPlatform_.getMap(mapType_, mergedMapUpdateArea_);
    }

    public void updateMap() {
        pushJob(jobUpdateWholeMap);
    }

    public void updatePose() {
        synchronized (this) {
            if (updatingPose_) {
                return;
            }

            updatingPose_ = true;
        }

        pushJob(jobUpdatePose);
    }

    public void updateLaserScan() {
        synchronized (RPSlamwareSdpAgent.this) {
            if (updatingLaserScan_) {
                return;
            }

            updatingLaserScan_ = true;
        }

        pushJob(jobUpdateLaserScan);
    }

    public void updateRobotStatus() {
        synchronized (RPSlamwareSdpAgent.this) {
            if (updatingStatus_)
                return;
            updatingStatus_ = true;
        }
        pushJob(jobUpdateStatus);
    }

    public void updateMoveAction() {
        synchronized (RPSlamwareSdpAgent.this) {
            if (updatingMoveAction_)
                return;

            updatingMoveAction_ = true;
        }

        pushJob(jobUpdateMoveAction);
    }

    // robot move action command
    public void moveTo(Location location) {
        synchronized (this) {
            this.location_ = location;
        }
        pushJobHead(jobMoveTo);
    }

    // robot move action command
    public void moveToPose(Pose pose) {
        synchronized (this) {
            this.pose_ = pose;
        }
        pushJobHead(jobMoveToPose);
    }


    public void moveToHome() {
        pushJobHead(jobGoHome);
    }

    public void rotateTo(int degree) {
        synchronized (RPSlamwareSdpAgent.this) {
            int d = degree;
            if (degree > 180) {
                d = -(360 - degree);
            }

            float yaw = (float) (Math.PI / 180 * (d));
            Rotation rotation = new Rotation(yaw, 0.0F, 0.0F);

            targetRotation_ = rotation;
            if (rotating_)
                return;
            rotating_ = true;
        }
        pushJob(jobRotateTo);
    }

    public void rotate(int degree) {
        synchronized (RPSlamwareSdpAgent.this) {
            int d = degree;
            if (degree > 180) {
                d = -(360 - degree);
            }

            float yaw = (float) (Math.PI / 180 * (d));
            Rotation rotation = new Rotation(yaw, 0.0F, 0.0F);

            targetRotate_ = rotation;
            if (rotating_)
                return;
            rotating_ = true;
        }
        pushJob(jobRotate);
    }

    public void moveBy(MoveDirection direction) {
        synchronized (this) {
            this.direction_ = direction;
        }
        pushJob(jobMoveBy);
    }

    public void sweepSpot(Location location) {
        synchronized (this) {
            this.location_ = location;
        }
        pushJobHead(jobSweepSpot);
    }

    // clear map
    public void clearMap() {
        Csjlogger.debug("clearmap");
        pushJob(jobClearMap);
    }

    // robot map command
    public void updateWalls() {
        synchronized (RPSlamwareSdpAgent.this) {
            if (updatingWalls_)
                return;
            updatingWalls_ = true;
        }
        pushJob(jobUpdateWalls);
    }

    public void addWall(Line line) {
        synchronized (this) {
            wallsToAdd_.add(line);
            if (wallsToAdd_.size() != 1)
                return;
        }
        pushJob(jobAddWalls);
    }

    public void addWalls(Vector<Line> lines) {
        synchronized (RPSlamwareSdpAgent.this) {
            if (wallsToAdd_.isEmpty()) {
                wallsToAdd_ = lines;
            } else {
                wallsToAdd_.addAll(lines);
                return;
            }
        }
        pushJob(jobAddWalls);
    }

    public void clearWalls() {
        pushJob(jobClearWalls);
    }

    public void removeWallById(int wallId) {
        synchronized (this) {
            wallId_ = wallId;
        }
        pushJob(jobRemoveWallById);
    }

    public void startSweep() {
        pushJobHead(jobStartSweep);
    }

    public void goHome() {
        pushJobHead(jobGoHome);
    }

    public void cancelAllActions() {
        pushJobHead(jobCancelAllActions);
    }

    public void getRobotHealth() {
        pushJob(jobGetRobotHealth);
    }

    private synchronized void pushJobHead(Runnable job) {
        worker.pushHead(job);
    }

    private synchronized void pushJob(Runnable job) {
        worker.push(job);
    }

    private void onRequestError(Exception e) {
        synchronized (this) {
            worker.clear();
            mapData_.clear();
            sweepMapData_.clear();
            robotPlatform_ = null;
            connected_ = false;
        }
//        Csjlogger.error("connect to slam error, will post event", e);
        EventBus.getDefault().post(new ConnectionLostEvent(e.getMessage()));
    }

    /**
     * 停止循环送餐
     */
    public void stopCircleRun() {
        synchronized (this) {
            isCircleRunning = false;
            circleRunWithVt_ = true;
            worker.clear();
            cancelAllActions();
        }
    }


    public void fakeWakeup() {
        try {
            AbstractSlamwarePlatform robotPlatform;

            synchronized (RPSlamwareSdpAgent.this) {
                robotPlatform = robotPlatform_;
            }
            if (robotPlatform == null) {
                return;
            }

            IMoveAction action = robotPlatform.goHome();
            action.cancel();
        } catch (Exception e) {
            Csjlogger.error(e);
            onRequestError(e);
        }

    }

    /**
     * puyz add
     */
    private boolean isCircleRunning = false;

    public void turnRound(final MoveDirection direction, final int count) {
        Runnable rurnRoundRunnable = new Runnable() {

            @Override
            public void run() {
                AbstractSlamwarePlatform robotPlatform;

                synchronized (RPSlamwareSdpAgent.this) {
                    robotPlatform = robotPlatform_;
                }
                if (robotPlatform == null) {
                    return;
                }

                int i = 0;
                try {
                    while (i < count * 20) {
                        i++;
                        IMoveAction action = robotPlatform.moveBy(direction);
                        Csjlogger.info("IMoveAction {}", action.getActionName());

                        try {
                            Thread.sleep(700);
                        } catch (InterruptedException e) {
                            e.printStackTrace();
                        }
                    }
                } catch (Exception e) {
                    Csjlogger.error(e);
                    onRequestError(e);
                }
            }
        };

        pushJob(rurnRoundRunnable);
    }

    private static class JobRotateToDegree implements Runnable {
        @Override
        public void run() {

        }
    }

    private class JobAutoUpdateMapping implements Runnable {

        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    return;
                }

                try {
                    platform.setMapUpdate(isUpdateMapping_);
                } catch (Exception e) {
                    Csjlogger.error(e);
                    onRequestError(e);
                }
            }
        }
    }


    private class JobRotate implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            Rotation targetRotate;

            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    rotating_ = false;
                    return;
                }
                targetRotate = targetRotate_;
            }

            try {
                platform.rotate(targetRotate);
            } catch (Exception e) {
                onRequestError(e);
            }
            rotating_ = false;
        }
    }

    private class JobCircleRun implements Runnable {
        /**
         * When an object implementing interface <code>Runnable</code> is used
         * to create a thread, starting the thread causes the object's
         * <code>run</code> method to be called in that separately executing
         * thread.
         * <p>
         * The general contract of the method <code>run</code> is that it may
         * take any action whatsoever.
         *
         * @see Thread#run()
         */
        @Override
        public void run() {
            Csjlogger.DBL();
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    return;
                }
            }

            IMoveAction moveAction = platform.getCurrentAction();
            if (moveAction != null) {
                moveAction.cancel();
            }


            // 设置参数 MoveOption
            MoveOption opt = new MoveOption();
            opt.setAppending(true);
            opt.setMilestone(true);

            if (circleRunWithVt_) {
                opt.setKeyPoints(true);
            }
            // 初始化判断变量
            int index = 0;
            int max = circle_.size();
            int lastPointSize = 0;
            // 第一个点
            Location location = circle_.get(index);

            IMoveAction action = platform.moveTo(location, opt);
            Csjlogger.debug("start circle run");
            Csjlogger.debug("isCircleRunning {}", isCircleRunning);
            while (isCircleRunning) {
                if (action != null) {
                    // 取剩余的点
                    int leftPointSize = action.getRemainingPath().getPoints().size();
                    int remainingMilestones = action.getRemainingMilestones().getPoints().size();

                    // 先判断任务是否被取消
                    Csjlogger.debug("leftPointSize is {} lastPointSize is {} , lastPointSize - leftPointSize  = {} ",
                            leftPointSize, lastPointSize, lastPointSize - leftPointSize);

                    if (leftPointSize > 0 && (lastPointSize - leftPointSize > 2)) {
                        // 理论上说 lastPointSize - leftPointSize 应该等于1（除了第一次）
                        // 但是有特殊情况，
                        // 如果大于2，则说明路径突然没有了，任务被取消了
                        // 继续走上一个点
                        action = platform.moveTo(location, opt);
                        Csjlogger.info("action cancled, continue goning to point");
                        // TODO 打印
                    } else {
                        if (remainingMilestones < 3) {
                            lastPointSize = leftPointSize;
                            // 剩余的点是否 <= 1，如果小于等于 1， 则需要走下面一个点了
                            if (leftPointSize <= 1) {
                                index++;
                                if (index >= max) {
                                    index = 0;
                                }

                                location = circle_.get(index);
                                action = platform.moveTo(location, opt);
                                Csjlogger.info("Go to next location, index = {}", index);
                            } else {
//                            EventBus.getDefault().post(new AutoRunPath(leftPointSize));
                                //TODO: 2017/3/30 打印信息
//                                Csjlogger.debug("剩余路径点 " + leftPointSize);
                                Csjlogger.warn("剩余路径点 {}", leftPointSize);
                            }
                        }
                    }
                } else {
//                    Csjlogger.error("action == null ");
                    isCircleRunning = false;
                    break;
                    //TODO 报错
                }

                try {
                    Thread.sleep(300);
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
            }
        }
    }

    private class JobSaveMap implements Runnable {
        public JobSaveMap() {
        }

        @Override
        public void run() {
//            Logger.d("chenqi saveMap");
            AbstractSlamwarePlatform robotPlatform;
            MapType mapType;
            synchronized (RPSlamwareSdpAgent.this) {
                robotPlatform = robotPlatform_;
                mapType = mapType_;
            }
            if (robotPlatform == null) {
                return;
            }
            try {
                RectF knownArea = robotPlatform.getKnownArea(mapType);
                Map map;
                map = robotPlatform.getMap(mapType, knownArea);
                MapBean mapBean = new MapBean();
                mapBean.setMap(map, FileUtil.SaveWalls(robotPlatform), FileUtil.SaveTrack(robotPlatform));
                FileUtil.saveMapToFile(mapBean);
            } catch (Exception e) {
//                Logger.d("chenqi  save failed");
                onRequestError(e);
                e.printStackTrace();
            }
        }
    }

    private class JobConnect implements Runnable {
        public JobConnect() {
        }

        @Override
        public void run() {
            try {
                String ip;
                int port;
                synchronized (RPSlamwareSdpAgent.this) {
                    ip = ip_;
                    port = port_;

                }
                if (ip == null || ip.isEmpty() || port <= 0 || port > 65535) {
                    onRequestError(new Exception());
                    return;
                }

                SlamwareCorePlatform robotPlatform = SlamwareCorePlatform.connect(ip, port);

                Pose robotPose = robotPlatform.getPose();
                List<MapType> mapTypes = robotPlatform.getAvailableMaps();
                MapType mapType = mapTypes.get(0);


                // RectF knownArea = robotPlatform.getKnownArea(mapType);
                LaserScan currentLaserScan = robotPlatform.getLaserScan();
                String sdpVersion = robotPlatform.getSlamwareVersion();

                shouldReadLocalizationQuliaty = false;

                String[] versions = sdpVersion.split("[.]");
                if (versions.length >= 2) {
                    if (Integer.valueOf(versions[0]) > 1) {
                        shouldReadLocalizationQuliaty = true;
                    } else if (Integer.valueOf(versions[0]) == 1 && Integer.valueOf(versions[1]) > 7) {
                        shouldReadLocalizationQuliaty = true;
                    } else if (versions.length >= 3 && Integer.valueOf(versions[0]) == 1 &&
                            Integer.valueOf(versions[1]) == 7 && Integer.valueOf(versions[2]) >= 1) {
                        shouldReadLocalizationQuliaty = true;
                    }
                }
                //不扫和定位地图
                robotPlatform.setMapUpdate(false);

                synchronized (RPSlamwareSdpAgent.this) {
                    robotPose_ = robotPose;
                    robotPlatform_ = robotPlatform;
                    robotSlamPlatform_ = robotPlatform;
                    mapType_ = mapType;
                    laserScan_ = currentLaserScan;
                    sdpVersion_ = sdpVersion;
                    connected_ = true;
                }
//                RectF clipArea = new RectF(
//                        (float) (robotPose.getLocation().getX() - SLAMWARE_MAX_UPDATE_MAP_RADIX * 2),
//                        (float) (robotPose.getLocation().getY() - SLAMWARE_MAX_UPDATE_MAP_RADIX * 2),
//                        (float) (robotPose.getLocation().getX() + SLAMWARE_MAX_UPDATE_MAP_RADIX * 2),
//                        (float) (robotPose.getLocation().getY() + SLAMWARE_MAX_UPDATE_MAP_RADIX * 2)
//                );
//                knownArea.intersect(clipArea);

                //updateMap(knownArea);
            } catch (Exception exception) {
                onRequestError(exception);
                return;
            }
//            Csjlogger.debug("connect slam success, will post event");
            EventBus.getDefault().post(new ConnectedEvent("success, SDK version is " + robotPlatform_.getSDKVersion()
                    + ", SlamwareVersion version is  " + robotPlatform_.getSlamwareVersion()));
        }
    }

    private class JobDisconnect implements Runnable {
        @Override
        public void run() {
            synchronized (RPSlamwareSdpAgent.this) {
                if (robotPlatform_ == null) {
                    return;
                }
                worker.clear();
                mapData_.clear();
                sweepMapData_.clear();
                robotPlatform_.disconnect();
                robotPlatform_ = null;
                connected_ = false;
            }
        }
    }

    /**
     * 恢复地图，这个方法是不管你有没有恢复的地图的情况下都会进行重新加载地图。
     * 所以谨慎使用。
     */
    private class JobRegainMap implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
            }
            if (platform == null || FileUtil.read() == null) {
                return;
            }
            try {
                MapBean map = (MapBean) FileUtil.read();//
                platform.setMap(map.getMap());
                platform.addLines(ArtifactUsage.ArtifactUsageVirutalWall, FileUtil.getWall(map));
                platform.addLines(ArtifactUsage.ArtifactUsageVirtualTrack, FileUtil.getTrack(map));
                //保存虚拟墙
//                Logger.d("chenqi  恢复成功" + map.getTimestamp() + map.getData());
            } catch (Exception e) {
//                Logger.d("chenqi 恢复地图");
                e.printStackTrace();
            }
            EventBus.getDefault().post(new MapOperationEvent(MapOperationEvent.REGAINMAPOK));
        }
    }

    private class JobUpdateMap implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            RectF area = new RectF();
            MapType mapType;

            synchronized (RPSlamwareSdpAgent.this) {
                RectF tmp = new RectF(mergedMapUpdateArea_);
                mergedMapUpdateArea_.set(area);
                area.set(tmp);
                platform = robotPlatform_;
                mapType = mapType_;
            }

            if (platform == null) {
                return;
            }

            if (area.isEmpty()) {
                EventBus.getDefault().post(new MapUpdateEvent(area));
                return;
            }

            Map map;

            try {
                map = platform.getMap(mapType, MapKind.EXPLORE_MAP, area);
            } catch (Exception e) {
//                Logger.d("chenqi -------------------" + e.getMessage());
                onRequestError(e);
                return;
            }

            synchronized (RPSlamwareSdpAgent.this) {
                mapData_.update(map);
            }

            EventBus.getDefault().post(new MapUpdateEvent(area));
        }
    }

    private class JobUpdateWholeMap implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            MapType mapType;

            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                mapType = mapType_;
            }

            if (platform == null) {
                return;
            }

            try {
                RectF area = platform.getKnownArea(mapType);
                updateMap(area);
            } catch (Exception e) {
                onRequestError(e);
            }
        }
    }

    private class JobUpdatePose implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    updatingPose_ = false;
                    return;
                }
            }

            Pose pose;

            try {
                pose = platform.getPose();
                synchronized (RPSlamwareSdpAgent.this) {
                    robotPose_ = pose;
                    updatingPose_ = false;
                }
            } catch (Exception e) {
                onRequestError(e);
                updatingPose_ = false;
                return;
            }
//            EventBus.getDefault().post(new RobotPoseUpdateEvent(pose));
        }
    }

    private class JobUpdateLaserScan implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    updatingLaserScan_ = false;
                    return;
                }
            }

            LaserScan laserScan;
            Pose pose = null;

            try {
                laserScan = platform.getLaserScan();
                if (laserScan.getPose() != null) {
                    pose = laserScan.getPose();
                }
            } catch (Exception e) {
                onRequestError(e);
                updatingLaserScan_ = false;
                return;
            }

            synchronized (RPSlamwareSdpAgent.this) {
                laserScan_ = laserScan;
                if (laserScan.getPose() != null && pose != null) {
                    robotPose_ = pose;
                }
                updatingLaserScan_ = false;
            }

            EventBus.getDefault().post(new LaserScanUpdateEvent(laserScan));
        }
    }

    private class JobUpdateStatus implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    updatingStatus_ = false;
                    return;
                }
            }

            int batteryPercentage;
            boolean isCharging;
            int localizationQuality;

            try {
                batteryPercentage = platform.getBatteryPercentage();
                isCharging = platform.getBatteryIsCharging();
                localizationQuality = shouldReadLocalizationQuliaty ? platform.getLocalizationQuality() : -1;
//                getRobotHealth();
            } catch (Exception e) {
                onRequestError(e);
                updatingStatus_ = false;
                return;
            }
            synchronized (RPSlamwareSdpAgent.this) {
                updatingStatus_ = false;
            }
            if (batteryPercentage > 100) {
                batteryPercentage = 100;
            }

            EventBus.getDefault().post(new RobotStatusUpdateEvent(batteryPercentage, isCharging, localizationQuality));
        }
    }

    private class JobUpdateMoveAction implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    updatingMoveAction_ = false;
                    return;
                }
            }

            IMoveAction moveAction;
            Path remainingMilestones = null;
            Path remainingPath = null;

            try {
                moveAction = platform.getCurrentAction();
                if (moveAction != null) {
                    if (OperateAction.shouldStop(moveAction.getActionName())) {
                        moveAction.cancel();
                        OperateAction.reset();
                    }
                    remainingMilestones = moveAction.getRemainingMilestones();
                    remainingPath = moveAction.getRemainingPath();
                }
            } catch (Exception e) {
                onRequestError(e);
                updatingMoveAction_ = false;
                return;
            }

            synchronized (RPSlamwareSdpAgent.this) {
                moveAction_ = moveAction;
                updatingMoveAction_ = false;
            }
//            EventBus.getDefault().post(new MoveActionUpdateEvent(remainingMilestones, remainingPath));
        }
    }


    private class JobChangeSpeed implements Runnable {
        public JobChangeSpeed() {

        }

        @Override
        public void run() {
            AbstractSlamwarePlatform platform = null;
            synchronized (this) {
                platform = robotPlatform_;
            }

            //   platform.setSystemParameter();
        }
    }

    /**
     *
     */
    private class JobMoveToPose implements Runnable {
        public JobMoveToPose() {
        }

        @Override
        public void run() {
            Pose pose;
            robotPlatform_ = DeviceManager.connect(ip_, port_);

            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                pose = pose_;
                platform = robotPlatform_;
            }

            if (pose == null || robotSlamPlatform_ == null) {
                return;
            }

            boolean isfinished = true;
            boolean isFirstActionDoing = true;
            int Times = 0;

            try {
                Location location = pose.getLocation();
                MoveOption moveOption = new MoveOption();
                moveOption.setKeyPoints(true);
                // TODO: 2017/4/12 验证这个方法
//                moveOption.setYaw(pose.getYaw());
                IAction action = platform.moveTo(location, moveOption);

                if (action != null) {
//                    Csjlogger.info("pose set to slam , action = {}, pose = {} , {}",
//                            action.getActionName(), String.valueOf(pose.getX()), String.valueOf(pose.getY()));
                }

                while (isFirstActionDoing) {
                    if (action != null) {
                        if (action.getStatus() == ActionStatus.ERROR) {
                            Thread.sleep(5000);
                            action = platform.moveTo(location, moveOption);
                        }

                        if (action.getStatus() != ActionStatus.FINISHED) {
                            Thread.sleep(1500);
                        } else {
                            isFirstActionDoing = false;
                        }
                    }
                }

                //到达之后转身
                IAction move2PoseAction = platform.rotateTo(pose.getRotation());

                if (move2PoseAction != null) {
//                    Csjlogger.info("arrive location, will rotateTo {}", String.valueOf(pose.getYaw()));
                }

                while (isfinished) {
                    // 这个是检查到达 Location 之后的转身
                    Thread.sleep(300);
                    if (Times < 3) {
                        if (move2PoseAction.getStatus() == ActionStatus.FINISHED || move2PoseAction.getStatus() == ActionStatus.ERROR) {
                            Times++;
                        } else if (move2PoseAction.getStatus() == ActionStatus.RUNNING) {
                            Times = 0;
                        }
                    } else if (Times > 3 || Times == 3) {
                        isfinished = false;
                        EventBus.getDefault().post(new ArriveHomeEvet());
                        Times = 0;
                    }
                    Thread.sleep(500);
                }
            } catch (Exception e) {
                onRequestError(e);
            }
        }
    }

    private class JobMoveTo implements Runnable {
        public JobMoveTo() {
        }

        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            Location location;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                location = location_;
                if (platform == null || location == null)
                    return;
            }


            try {
                IMoveAction action = platform.moveTo(location);
                Csjlogger.info("IMoveAction  {} , will moveto  {}; {},", action.getActionName(), String.valueOf(location.getX())
                        , String.valueOf(location.getY()));
            } catch (Exception e) {
                onRequestError(e);
            }
        }
    }

    private class JobSweepSpot implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            Location location;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                location = location_;
                if (platform == null || location == null) {
                    return;
                }
            }

            try {
                platform.sweepSpot(location);
            } catch (Exception e) {
                onRequestError(e);
            }
        }
    }

    private class JobRotateTo implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            Rotation targetRotation;

            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    rotating_ = false;
                    return;
                }
                targetRotation = targetRotation_;
            }

            try {
                platform.rotateTo(targetRotation);
            } catch (Exception e) {
                onRequestError(e);
            }
            rotating_ = false;
        }
    }

    private class JobMoveBy implements Runnable {
        public JobMoveBy() {
        }

        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            MoveDirection direction;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                direction = direction_;
                if (platform == null || direction == null) {
                    return;
                }
            }
            try {
                IMoveAction action = platform.moveBy(direction);
                Csjlogger.info("IMoveAction {}", action.getActionName());
            } catch (Exception e) {
                onRequestError(e);
            }

            synchronized (RPSlamwareSdpAgent.this) {
                direction_ = null;
            }
        }
    }

    private class JobClearMap implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;

            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
            }

            if (platform == null)
                return;

            try {
                platform.clearMap();
                Csjlogger.info("Clear map");
            } catch (Exception e) {
                onRequestError(e);
                return;
            }

            synchronized (RPSlamwareSdpAgent.this) {
                mapData_.clear();
                sweepMapData_.clear();
            }

//            EventBus.getDefault().post(new MapUpdateEvent(new RectF(-60, -60, 60, 60)));
        }
    }

    private class JobUpdateWalls implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    updatingWalls_ = false;
                    return;
                }
            }

            Vector<Line> walls;

            try {
                walls = platform.getWalls();
            } catch (Exception e) {
                onRequestError(e);
                updatingWalls_ = false;
                return;
            }

            synchronized (RPSlamwareSdpAgent.this) {
                walls_ = walls;
            }

            updatingWalls_ = false;
            EventBus.getDefault().post(new WallUpdateEvent(walls));
        }
    }

    private class JobAddWalls implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            Vector<Line> wallsToAdd = new Vector<>();

            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null)
                    return;
                Vector<Line> tmp = new Vector<>(wallsToAdd_);
                wallsToAdd_.clear();
                wallsToAdd.clear();
                wallsToAdd.addAll(tmp);
            }

            if (wallsToAdd.isEmpty())
                return;

            try {
                platform.addWalls(wallsToAdd);
            } catch (Exception e) {
                onRequestError(e);
            }

            updateWalls();
        }
    }

    private class JobClearWalls implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null)
                    return;
            }
            try {
                platform.clearWalls();
            } catch (Exception e) {
                onRequestError(e);
            }

            updateWalls();
        }
    }

    private class JobRemoveWallById implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            int wallId;

            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                if (platform == null) {
                    return;
                }
                wallId = wallId_;
            }

            try {
                platform.clearWallById(wallId);
            } catch (Exception e) {
                onRequestError(e);
            }

            updateWalls();
        }
    }

    private class JobStartSweep implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
            }

            if (platform == null)
                return;

            try {
                platform.startSweep();
            } catch (Exception e) {
                onRequestError(e);
            }
        }
    }


    private class JobGoHomePose implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            Pose pose;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
                pose = pose_;
            }

            if (platform == null || pose == null) {
                return;
            }
            boolean isfinished = true;
            int Times = 0;
//            Logger.d("Move To Pose is run");
            try {
                IAction action = platform.goHome();
                action.waitUntilDone();
                IAction goHome = platform.rotateTo(pose.getRotation());
                while (isfinished) {
                    Thread.sleep(300);
                    if (Times < 3) {
                        if (goHome.getStatus() == ActionStatus.FINISHED) {
                            Times++;
                        } else if (goHome.getStatus() == ActionStatus.RUNNING) {
                            Times = 0;
                        }
                    } else if (Times > 3 || Times == 3) {
                        isfinished = false;
                        EventBus.getDefault().post(new ArriveHomeEvet());
                        Times = 0;
//                        Logger.d("chenqi run is not or is over" + goHome.getStatus().toString());
                    }
                    // todo  print move2PoseAction.getStatus().toSting();
                    Thread.sleep(500);
                }
            } catch (Exception e) {
                onRequestError(e);
            }
        }
    }

    private class JobGoHome implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
            }
            if (platform == null)
                return;

            try {
                platform.goHome();
                IAction action = platform.goHome();
            } catch (Exception e) {
                onRequestError(e);
            }

        }
    }

    private class JobCancelAllActions implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            IMoveAction moveAction;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
            }
            if (platform == null)
                return;

            try {
                moveAction = platform.getCurrentAction();
                if (moveAction != null) {
                    moveAction.cancel();
                }
            } catch (Exception e) {
                onRequestError(e);
            }

            worker.clear();

            synchronized (RPSlamwareSdpAgent.this) {
                updatingStatus_ = false;
                updatingMoveAction_ = false;
                updatingLaserScan_ = false;
                updatingPose_ = false;
                updatingWalls_ = false;
                rotating_ = false;
            }

            EventBus.getDefault().post(new MapUpdateEvent(new RectF()));
        }
    }

    private class JobGetRobotHealth implements Runnable {
        @Override
        public void run() {
            AbstractSlamwarePlatform platform;
            synchronized (RPSlamwareSdpAgent.this) {
                platform = robotPlatform_;
            }

            if (platform == null) {
                return;
            }

            try {
                HealthInfo info = platform.getRobotHealth();
                EventBus.getDefault().post(new RobotHealthInfoEvent(info));
            } catch (Exception e) {
                onRequestError(e);
            }
        }
    }
}
